#ifndef SR300B_HPP
#define SR300B_HPP

#include "sr300a.hpp"

#include "urcontroloffseta.hpp"

class SR300B : public SR300A
{
public:
    cv::Vec6d fingerToHead;
    cv::Vec6d cameraToFinger;

public:
    SR300B()
        : SR300A()
        , fingerToHead(0,0,1.217,0,0,0)
        , cameraToFinger(-0.016,-0.033,-0.25,0,0,0)
    {

    }

    cv::Vec6d ToGlobalPos(cv::Vec3f cameraPos, cv::Vec6d armPos, double zDrop=0.1)
    {
        cv::Vec3d ptCam(cameraPos[0],cameraPos[1],cameraPos[2]);
        //        std::cout<<"ptCam="<<ptCam<<"\n"<<std::flush;
        cv::Vec3d ptFinger=URControlOffset::HeadToBase(ptCam, cameraToFinger);
        //        std::cout<<"ptFinger="<<ptFinger<<"\n"<<std::flush;
        cv::Vec6d oldToNewFinger(ptFinger[0], ptFinger[1],zDrop,0,0,0);
        cv::Vec6d pos=URControlOffset::ComposeTransform(URControlOffset::ComposeTransform(URControlOffset::ComposeTransform(URControlOffset::ReverseTransform(fingerToHead),oldToNewFinger),fingerToHead),armPos);
        return pos;
    }


    void CrossPosGlobal(std::vector<cv::Vec6d> &crossPos, const cv::Mat &xyz, cv::Vec6d armPos, double zDrop=0.04)
    {
        std::vector<cv::Vec3f> cross;
        CrossLocate(cross, xyz, 0.28, 0.01, cv::Size(130,130), 0.05);
        crossPos.clear();
        for(auto crosspt:cross)
        {
            cv::Vec6d pos=ToGlobalPos(crosspt,armPos,zDrop);
            crossPos.push_back(pos);
        }
    }



    virtual void ProcessFrame(const cv::Mat &color, const cv::Mat &ir, const cv::Mat &depth, const cv::Mat &xyz, const cv::Mat &texture, const cv::Mat &aligned_depth, const cv::Mat &aligned_xyz)
    {
        SR300::ProcessFrame(color, ir, depth, xyz, texture, aligned_depth, aligned_xyz);

        char ipUR[]="192.168.1.13";
        char ipPC[]="192.168.1.14";

        cv::Vec6d p;
        URControl::GetTCPPose(ipUR,ipPC, p[0],p[1],p[2],p[3],p[4],p[5]);
        printf("[%g,%g,%g,%g,%g,%g]\n",p[0],p[1],p[2],p[3],p[4],p[5]);

        std::vector<cv::Vec6d> crossPos;
        CrossPosGlobal(crossPos, xyz, p);


        for(auto pos:crossPos)
        {
            URControl::SetTCPPose(ipUR,ipPC, pos[0],pos[1],pos[2]-0.02,pos[3],pos[4],pos[5],0.2,0.3);
            URControl::SetTCPPose(ipUR,ipPC, pos[0],pos[1],pos[2],pos[3],pos[4],pos[5], 0.2,0.3);
            URControl::SetTCPPose(ipUR,ipPC, pos[0],pos[1],pos[2]-0.02,pos[3],pos[4],pos[5],0.2,0.3);
//            k=cv::waitKey(3000);
        }

//        URControl::SetTCPPose(ipUR,ipPC, p[0],p[1],p[2],p[3],p[4],p[5]);
    }




};


#endif
